/** \file   serialprintf.c
 *
 *  \brief  This file contains
 *
 *   This file performs functions on the following:
 *
 *      1. UNDEFINED.
 *
 *   Note: N/A
 *
 *  \author     Jiawei Xie
 *
 *  \version    0.1   Created newly for NAVCON
 */


#include "funcdef.h"

/*
 * External references for serial_printf
 */



//system time stamp
extern short TIME_STAMP_H;
extern short TIME_STAMP_L;
/*
 * Starting message printing string
 * Note: Buffer alignement is required only when working in DMA Mode.
 */
#define SERIAL_BUFFER_SIZE 		256
#pragma DATA_ALIGN(serial_buffer, SERIAL_BUFFER_SIZE);
char serial_buffer[SERIAL_BUFFER_SIZE];
/* Tx memory buffer */
#pragma DATA_ALIGN(Uart_TxBuffer, SERIAL_BUFFER_SIZE);
static Int8  Uart_TxBuffer[SERIAL_BUFFER_SIZE];

int DATA_LOG_TYPE = TXT_DATA_LOG;
//Int16 short
static short bin_serial_buf[SERIAL_BUFFER_SIZE];

int start_index,end_index,remaining = 0;
int txbytes = 0;
int dec_count = 0;
int gps_count = 0;
/* ========================================================================== */
/*                        FUNCTION DEFINTIONS                                 */
/* ========================================================================== */

void process_buffer(TSK_Ctrl_b * tsk, int type);
void serial_output( char* format, ... );
void serial_start_sending_data(TSK_Ctrl_b * tsk, char* buf, unsigned int len);
void endian_swap(void);

/*
 * NOTE: serial print maximum run at 45.5Hz with 83 char payload.
 * transfer maximum ~ 30212 bit/second
 * around 33% if using 115200 baud rate (11520 bytes)
 */
void datalog_tsk(void)
{

	TSK_Ctrl_b *self = &(SCB.TCB[TSK_datalog]);

	for(;;){
		SEM_pend(&datalog_sem, SYS_FOREVER);
		process_buffer(self,DATA_LOG_TYPE);
		TSK_yield();
	}
}


void process_buffer(TSK_Ctrl_b * tsk, int type){

	int i,j = 0;
	int nchars = 0;
	int bin_tx_bytes = 0;
	int len = 0;
	char check_sum = 0;
	char temp[2] = {0};

	if(type == TXT_DATA_LOG){

		nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE, "%d:%d:%d:%d:%d:%d:%d:%d:%d:%d:%d",
				vNavData.rGyro_x,vNavData.rGyro_y,vNavData.rGyro_z,vNavData.rGyro_T,
				vNavData.rBMA180_x,vNavData.rBMA180_y,vNavData.rBMA180_z,vNavData.rBMA180_T,
				vNavData.rMag_x,vNavData.rMag_y,vNavData.rMag_z);

		for(i=0;i<nchars-1;i++){
			check_sum += serial_buffer[i];
		}
		serial_buffer[nchars-1] = '\r';
		serial_buffer[nchars] = '\n';
		serial_buffer[nchars+1] = '\0';
		txbytes = strlen(serial_buffer);
		serial_start_sending_data(tsk, (char*)&serial_buffer, txbytes);


	}else if (type == BIN_DATA_LOG){
		//update GPS counter
		gps_count++;
		bin_serial_buf[0] = 0xFF81;
		bin_serial_buf[1] = SCB.SYSTEM_TIME_H;
		bin_serial_buf[2] = SCB.SYSTEM_TIME_L;
		bin_serial_buf[3] = vNavData.rGyro_x;
		bin_serial_buf[4] = vNavData.rGyro_y;
		bin_serial_buf[5] = vNavData.rGyro_z;
		bin_serial_buf[6] = vNavData.rGyro_T;
		bin_serial_buf[7] = vNavData.rBMA180_x;
		bin_serial_buf[8] = vNavData.rBMA180_y;
		bin_serial_buf[9] = vNavData.rBMA180_z;
		bin_serial_buf[10] = vNavData.rBMA180_T;
		bin_serial_buf[11] = vNavData.rMag_x;
		bin_serial_buf[12] = vNavData.rMag_y;
		bin_serial_buf[13] = vNavData.rMag_z;
		bin_serial_buf[14] = (short)(((long long)(vNavData.P * 1000)>>16) & 0xFFFF);
		bin_serial_buf[15] = (short)(((long long)(vNavData.P * 1000)) & 0xFFFF);
		bin_serial_buf[16] = (short)(((long long)(vNavData.T * 1000)>>16) & 0xFFFF);
		bin_serial_buf[17] = (short)(((long long)(vNavData.T * 1000)) & 0xFFFF);
		//the last short must only have 8 bit written, the last 8 bit will fill with checksum
		bin_serial_buf[18] = (ACC_CURRENT_SCALE_INDEX & 0xF) << 4 | (MAG_CURRENT_SCALE_INDEX & 0xF);
		bin_tx_bytes = 19;
		for(i=1;i<bin_tx_bytes;i++){
			temp[0] = bin_serial_buf[i] >> 8;
			temp[1] = bin_serial_buf[i] & 0xFF;
			check_sum += temp[0];
			check_sum += temp[1];
		}
		bin_serial_buf[bin_tx_bytes-1] = (((bin_serial_buf[bin_tx_bytes-1]) & 0xFF) << 8)|(check_sum & 0xFF);
		txbytes = 2*bin_tx_bytes;
		endian_swap();
		serial_start_sending_data(tsk, (char*)&bin_serial_buf, txbytes);


		if(gps_count%10 ==0){
			check_sum = 0;
			bzeros((unsigned char*)bin_serial_buf,2*SERIAL_BUFFER_SIZE);
			/* GPS information */
			gps_count = 0;
			bin_serial_buf[0] = 0xFF83;
			/*bin_serial_buf[1] = (short)((vNavData.GPS_BUF.iTOW >> 16) & 0xFFFF);
			bin_serial_buf[2] = (short)((vNavData.GPS_BUF.iTOW) & 0xFFFF);
			bin_serial_buf[3] = (short)((vNavData.GPS_BUF.ecefX >> 16) & 0xFFFF); 	//vNavData.GPS_BUF.ecefX_H;
			bin_serial_buf[4] = (short)(vNavData.GPS_BUF.ecefX & 0xFFFF); 		   	//vNavData.GPS_BUF.ecefX_L;
			bin_serial_buf[5] = (short)((vNavData.GPS_BUF.ecefY >> 16) & 0xFFFF); 	//vNavData.GPS_BUF.ecefY_H;
			bin_serial_buf[6] = (short)(vNavData.GPS_BUF.ecefY & 0xFFFF); 			//vNavData.GPS_BUF.ecefY_L;
			bin_serial_buf[7] = (short)((vNavData.GPS_BUF.ecefZ >> 16) & 0xFFFF); 	//vNavData.GPS_BUF.ecefZ_H;
			bin_serial_buf[8] = (short)((vNavData.GPS_BUF.ecefZ) & 0xFFFF);
			bin_serial_buf[9] = (short)((vNavData.GPS_BUF.pAcc >> 16) & 0xFFFF); 	//vNavData.GPS_BUF.pAcc_H;
			bin_serial_buf[10] = (short)((vNavData.GPS_BUF.pAcc) & 0xFFFF); 		//vNavData.GPS_BUF.pAcc_L;
			bin_serial_buf[11] = (short)((vNavData.GPS_BUF.ecefVX >> 16) & 0xFFFF); //vNavData.GPS_BUF.ecefVX_H;
			bin_serial_buf[12] = (short)((vNavData.GPS_BUF.ecefVX) & 0xFFFF); 		//vNavData.GPS_BUF.ecefVX_L;
			bin_serial_buf[13] = (short)((vNavData.GPS_BUF.ecefVY >> 16) & 0xFFFF); //vNavData.GPS_BUF.ecefVY_H;
			bin_serial_buf[14] = (short)((vNavData.GPS_BUF.ecefVY) & 0xFFFF); 		//vNavData.GPS_BUF.ecefVY_L;
			bin_serial_buf[15] = (short)((vNavData.GPS_BUF.ecefVZ >> 16) & 0xFFFF); //vNavData.GPS_BUF.ecefVZ_H;
			bin_serial_buf[16] = (short)((vNavData.GPS_BUF.ecefVZ) & 0xFFFF); 		//vNavData.GPS_BUF.ecefVZ_L;
			bin_serial_buf[17] = (short)((vNavData.GPS_BUF.sAcc >> 16) & 0xFFFF); 	//vNavData.GPS_BUF.sAcc_H;
			bin_serial_buf[18] = (short)((vNavData.GPS_BUF.sAcc) & 0xFFFF); 		//vNavData.GPS_BUF.sAcc_L;
			bin_serial_buf[19] = vNavData.GPS_BUF.numSV; */

			if(vNavData.gps_data.length > 0){
				len = vNavData.gps_data.length/2;
				for(i = 1,j=0; i<len+1; i++)
				{
					 bin_serial_buf[i] = (vNavData.gps_data.data[j] << 8) | (vNavData.gps_data.data[j+1]);
					 j = j + 2;
				}
			}
			bin_tx_bytes = len+1;
			/* Calculate checksum */
			for(i=1;i<bin_tx_bytes;i++){
				temp[0] = bin_serial_buf[i] >> 8;
				temp[1] = bin_serial_buf[i] & 0xFF;
				check_sum += temp[0];
				check_sum += temp[1];
			}

			bin_serial_buf[bin_tx_bytes] = (short)check_sum;
			bin_serial_buf[bin_tx_bytes] = bin_serial_buf[bin_tx_bytes] & 0x00FF;

			txbytes = 2*(bin_tx_bytes+1); // MESSAGE LENGTH: 20 shorts = 40 bytes
			endian_swap();
			serial_start_sending_data(tsk, (char*)&bin_serial_buf, txbytes);
		}

	}else if(type == FULL_SET_SCALE_DATA_LOG){

		txbytes = 30;
		if(remaining > txbytes){
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
		}else if(remaining == 0){
			bzeros((unsigned char*)serial_buffer,SERIAL_BUFFER_SIZE);
			nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE,
					"%d Gx%.3f Gy%.3f Gz%.3f Gt%.3f At%.3f Rol%.3f Pit%.3f A%.3f P%.3f T%.3f H%.3f",
					SCB.SYSTEM_TIME, vNavData.Gyro_x,vNavData.Gyro_y,vNavData.Gyro_z,vNavData.Gyro_T,
					vNavData.BMA180_T,vNavData.roll,vNavData.pitch,vNavData.azimuth,
					vNavData.P,vNavData.T,vNavData.BAROH_R);
			serial_buffer[nchars-1] = '\r';
			serial_buffer[nchars] = '\n';
			serial_buffer[nchars+1] = '\0';
			//update start_index and remaining bytes
			start_index = 0;
			remaining = strlen(serial_buffer);
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
		}else{
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], remaining);
			remaining = 0;
		}
	}else if(type == GPS_DATA_LOG){

		txbytes = 30;
		if(remaining > txbytes){
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
		}else if(remaining == 0){
			bzeros((unsigned char*)serial_buffer,SERIAL_BUFFER_SIZE);
			nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE,
					"%lu EX %d EY %d EZ %d VX %d VY %d VZ %d pA %d S %d F %d ",
					SCB.SYSTEM_TIME,
					vNavData.GPS_BUF.ecefX,vNavData.GPS_BUF.ecefY,vNavData.GPS_BUF.ecefZ,
					vNavData.GPS_BUF.ecefVX, vNavData.GPS_BUF.ecefVY, vNavData.GPS_BUF.ecefVZ,
					vNavData.GPS_BUF.pAcc, (Uint32)vNavData.GPS_BUF.numSV, (Uint32)vNavData.GPS_BUF.gpsFix);
			serial_buffer[nchars-1] = '\r';
			serial_buffer[nchars] = '\n';
			serial_buffer[nchars+1] = '\0';
			//update start_index and remaining bytes
			start_index = 0;
			remaining = strlen(serial_buffer);
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], txbytes);
			remaining = remaining - txbytes;
			start_index = start_index + txbytes;
		}else{
			serial_start_sending_data(tsk,(char*)&serial_buffer[start_index], remaining);
			remaining = 0;
		}


	}else if(type == SYSTEM_PROFILING){

		if(dec_count == 50){
			//run at 50Hz/50 = 1Hz
			nchars = snprintf (serial_buffer, SERIAL_BUFFER_SIZE,
					"TSK_BMA180 %.3f\r\nTSK_ITG3205 %.3f\r\nTSK_HMC5883L %.3f\r\nTSK_MS5611 %.3f\r\nTSK_GPS %.3f\r\nMATRIX_TEST %.3f",
					SCB.TCB[TSK_BMA180].avg_t,SCB.TCB[TSK_ITG3205].avg_t,SCB.TCB[TSK_HMC5883L].avg_t,
					SCB.TCB[TSK_MS5611].avg_t,SCB.TCB[TSK_GPS].avg_t,SCB.TCB[MATRIX_TEST].avg_t);

			serial_buffer[nchars-1] = '\r';
			serial_buffer[nchars] = '\n';
			serial_buffer[nchars+1] = '\0';
			txbytes = strlen(serial_buffer);
			serial_start_sending_data(tsk, (char*)&serial_buffer, txbytes);
			dec_count = 0;
		}else{
			dec_count++;
		}

	}
}



void endian_swap(void)
{
	int i = 0;
	short temp = 0;
	for(i=0; i<(txbytes/2);i++){
		temp = (bin_serial_buf[i] >> 8) & 0x00FF;
		bin_serial_buf[i] = temp|(bin_serial_buf[i]<<8);
	}
}

// add this text to the output buffer
void serial_output( char* format, ... )
{
	va_list arglist ;
	va_start(arglist, format) ;

	int start_index = end_index ;
	int remaining = SERIAL_BUFFER_SIZE - start_index ;

	if (remaining > 1)
	{
		int wrote = vsnprintf( (char*)(&serial_buffer[start_index]), (size_t)remaining, format, arglist) ;
		end_index = start_index + wrote;
	}

	va_end(arglist);

	return ;
}

//start UART transaction
void serial_start_sending_data(TSK_Ctrl_b *tsk, char* buf, unsigned int len)
{
    Int     status = 0;
    memcpy(Uart_TxBuffer,buf,len);

	if(SEM_pendBinary(&UART2_OUT_SEM,SYS_TIME_OUT)){

		status = GIO_submit(tsk->outhd,IOM_WRITE, Uart_TxBuffer, &len, NULL);
		SEM_postBinary(&UART2_OUT_SEM);

	}else{
		//time out
		LOG_printf(tsk->trace, "UART2 Timeout!\n");
		status = -1;
	}

    if(!((status == IOM_COMPLETED)))
    {
        LOG_printf(tsk->trace, "\r\n Error from serial_start_sending_data Code %d\n",status);
    }
}

/* ========================================================================== */
/*                              END OF FILE                                   */
/* ========================================================================== */
